﻿using System;
using System.Drawing;
using System.Windows.Forms;
using LDriver;
using System.Runtime.InteropServices;
using LDriver.Protocols;


namespace LClient
{

    public partial class ucLidarVisualizer : UserControl
    {

        // Elenco coordinate
        // La coordinata ix400 è relativa alla posizione del mouse
        private Coordinates[] Coords = new Coordinates[401];
        private int ixCoords = -1;
        private int MaxIxCoords = -1; // Valore massimo dell'indice, calcolato sulla nuova rilevazione

        // Dimensione della spazio destinato al disegno.
        // E' la dimensione della grpView
        private Size RadarSize = new Size(0, 0);
        // E' la posizione della grpView
        private Point RadarLocation = new Point(0, 0);

        // Valori 0 delle coordinate, sono il centro del disegno, escludendo il pan
        private Point Center = new Point(0, 0);

        // Lato più corto del contenitore del radar
        int MaxDistance = 0;

        // Posizione del mouse, utilizzata per tirare una linea e visualizzare le informazioni
        // dei punti
        private Double MouseAngle = 0;

        // Variabile per il disegno
        private bool isDrawing = false;

        // Variabili per gli alerts
        double alertFromAngle = -1;
        double alertToAngle = -1;
        int alertMinDistance = 0;
        bool alertCrossZero = false;

        // Variabili per le stats
        double statsMaxDistance = double.MinValue;
        double statsMinDistance = double.MaxValue;
        double statsCountDistances = 0;
        double statsSumDistances = 0;

        double statsMaxDistanceO = double.MinValue;
        double statsMinDistanceO = double.MaxValue;
        double statsCountDistancesO = 0;
        double statsSumDistancesO = 0;

        // Penne e colori
        Pen pInternalCircle;
        Pen pExternalCircle;
        Pen pInternalLine;
        Pen pInternalFullLine;
        Pen pAlertLine;
        Pen pMouse;
        SolidBrush bBackgroundColor;
        SolidBrush bDegrees;
        Font fDefault;

        // Zoom
        private decimal ZoomFactor = 1;

        // Oggetti per gestire correttamente il disegno BitBlt
        private Bitmap memBmp;
        private Bitmap bmpRadar;
        private Graphics memDC;

        public ucLidarVisualizer()
        {
            // Inizializzazione
            InitializeComponent();

            // Layout colori
            pExternalCircle = new Pen(Color.Lime); // Colore del cerchio esterno
            pInternalCircle = new Pen(Color.Gray); // Colore dei cerchi interni
            pInternalCircle.DashPattern = new float[] { 5, 5 }; // Tratteggio
            pInternalLine = new Pen(Color.Gray); // Colore delle linee interne, linea tratteggiata
            pInternalLine.DashPattern = new float[] { 2, 10 }; // Tratteggio
            pInternalFullLine = new Pen(Color.Gray); // Colore delle linee interne, linea piena
            pAlertLine = new Pen(Color.Red); // Linea dell'alert
            pAlertLine.DashPattern = new float[] { 3, 3 }; // Tratteggio
            pMouse = new Pen(Color.Yellow); // Linea del mouse

            bBackgroundColor = new SolidBrush(Color.Black); // Radar background color
            bDegrees = new SolidBrush(Color.Lime); // Colore dei gradi

            fDefault = new Font("Microsoft Sans Serif", 9);

            // Funzione per calcolo dimensioni ed altro
            ResizeUC();

            SetAlert(20,70,100);

        }

        public void SetAlert(double FromAngle, double ToAngle, int MinDistance)
        {
            alertFromAngle = FromAngle;
            alertToAngle = ToAngle;
            alertMinDistance = MinDistance;

            // Segno se l'angolo passa lo zero (es. 350 > 10) oppure non
            // passa lo zero (es. 45 > 90)
            alertCrossZero = (alertFromAngle > alertToAngle);

        }

        public void ResizeUC()
        {
            // Memorizzo in variabili le dimensioni e le coordinate della vista 
            // dedicata al radar
            RadarLocation = grpView.Location;
            RadarSize = grpView.Size;

            // Calcolo il punto corrispondende al centro del radar.
            // DA questo punto partiranno tutte le linee ed i punti
            Center = new Point(
                (RadarSize.Width / 2),
                (RadarSize.Height / 2));

            // Recupero il lato più corto dello spazo dedicato al radar e lo 
            // riduco di un certo margine per andare a piazzare i punti ed i cerchi
            MaxDistance = (RadarSize.Width < RadarSize.Height ? RadarSize.Width : RadarSize.Height) - 50;

            // La bitmap di destinazione ha dimensione pari al RadarSize
            memBmp = new Bitmap(RadarSize.Width, RadarSize.Height);
            Graphics clientDC = this.CreateGraphics();
            IntPtr hdc = clientDC.GetHdc();
            IntPtr memdc = Win32Support.CreateCompatibleDC(hdc);
            Win32Support.SelectObject(memdc, memBmp.GetHbitmap());
            memDC = Graphics.FromHdc(memdc);
            clientDC.ReleaseHdc(hdc);

            // Creo l'immagine di base del radar
            CreateBase();
            Draw();

        }

        private void CreateBase()
        {

            // Creo la bitmap che conterrà il radar base
            Graphics myGraphics = this.CreateGraphics();
            bmpRadar = new Bitmap(RadarSize.Width, RadarSize.Height, myGraphics);
            Graphics memoryGraphics = Graphics.FromImage(bmpRadar);

            // Radar background
            memoryGraphics.FillRectangle(bBackgroundColor, 0, 0, RadarSize.Width, RadarSize.Height);

            // Gestisco l'immagine del drone, che di base è 75x75
            if (chkShowDrone.Checked)
            {
                memoryGraphics.DrawImage(
                    picDrone.BackgroundImage,
                    Center.X - (int)(picDrone.Width / 2 * ZoomFactor),
                    Center.Y - (int)(picDrone.Height / 2 * ZoomFactor),
                    (int)(picDrone.Width * ZoomFactor),
                    (int)(picDrone.Height * ZoomFactor));
            }

            // Radar border
            Point[] border =
             {
                 new Point(2, 2),
                 new Point(2, RadarSize.Height-4),
                 new Point(RadarSize.Width-4,  RadarSize.Height-4),
                 new Point(RadarSize.Width-4, 2),
                 new Point(2,  2)
             };

            memoryGraphics.DrawLines(new Pen(Color.White), border);

            double rad = 0;
            int X = 0;
            int Y = 0;

            // Linee tratteggiate e numeri
            for (int angle = 0; angle < 360; angle += 5)
            {
                // Calcolo del radiante
                rad = (angle * Math.PI / 180.0);

                // Calcolo della posizione finale del punto
                X = (int)(Math.Sin(rad) * (MaxDistance / 2) + Center.X);
                Y = (int)(Center.Y - Math.Cos(rad) * (MaxDistance / 2));

                // Se sono un multiplo di 30 allora disegno la riga dal centro,
                // in caso contrario piazzo solo un trattino
                if (angle % 30 == 0)
                {
                    memoryGraphics.DrawLine(pInternalLine, Center.X, Center.Y, X, Y);
                    memoryGraphics.DrawString(
                        angle.ToString() + "°",
                        fDefault,
                        bDegrees,
                        new Point(
                            (angle >= 210 && angle <= 330 ? X - 30 : X),
                            (angle >= 300 || angle <= 60 ? Y - 15 : Y)
                        )
                    );

                }
                else
                {
                    // Calcolo della posizione iniziale del trattino
                    int X2 = (int)(Math.Sin(rad) * ((MaxDistance - 15) / 2) + Center.X);
                    int Y2 = (int)(Center.Y - Math.Cos(rad) * ((MaxDistance - 15) / 2));

                    memoryGraphics.DrawLine(pInternalFullLine, X, Y, X2, Y2);

                }

            }

            // Cerchi concentrici, disegnati solamente se non sono in fase di pan
            for (int i = 0; i < MaxDistance; i += (int)(100 * ZoomFactor))
            {
                memoryGraphics.DrawEllipse(pInternalCircle, (Center.X) - (i / 2), (Center.Y) - (i / 2), i, i);

            }
            memoryGraphics.DrawEllipse(pExternalCircle, (Center.X) - (MaxDistance / 2), (Center.Y) - (MaxDistance / 2), MaxDistance, MaxDistance);


            // Disegno le info dell'alert se presente
            if (!(alertFromAngle == -1))
            {
                // Disegno le due linee relative all'aler
                DrawLineFromCenter(alertFromAngle, pAlertLine, memoryGraphics, false);
                DrawLineFromCenter(alertToAngle, pAlertLine, memoryGraphics, false);

                // Disegno l'arco relativo all'alert.
                // ATTENZIONE! Il grado 0 dell'arco corrisponde al grado 90 del radar
                memoryGraphics.DrawArc(pAlertLine,
                    Center.X - (int)((alertMinDistance * ZoomFactor)),
                    Center.Y - (int)((alertMinDistance * ZoomFactor)),
                    (int)(alertMinDistance * ZoomFactor) * 2,
                    (int)(alertMinDistance * ZoomFactor) * 2,
                    (int)(alertFromAngle - 90),
                    (int)(alertToAngle - alertFromAngle));

            }

        }

        private void DrawLineFromCenter(double angle, Pen p, Graphics memoryGraphics, bool write)
        {
            double rad = 0;
            int X = 0;
            int Y = 0;

            // Calcolo del radiante
            rad = (angle * Math.PI / 180.0);

            // Calcolo della posizione finale del punto
            X = (int)(Math.Sin(rad) * (MaxDistance / 2) + Center.X);
            Y = (int)(Center.Y - Math.Cos(rad) * (MaxDistance / 2));

            memoryGraphics.DrawLine(p, Center.X, Center.Y, X, Y);

            if (write)
            {
                memoryGraphics.DrawString(angle.ToString() + "°", fDefault,
                    new SolidBrush(p.Color), new Point(X, Y));
            }

        }

        private void Draw()
        {
            // Variabile globale indicante che sono in fase di disegno.
            // Impedisce ulteriori chiamate alla procedura corrente fino a quando non è
            // finita.
            isDrawing = true;

            // Ripulisco l'intero disegno dedicato al radar ed 
            // incollo la bitmap del radar creata in precedenza
            memDC.FillRectangle(bBackgroundColor, 0, 0, RadarSize.Width, RadarSize.Height);
            memDC.DrawImage(bmpRadar, 0, 0);

            // Variabile di controllo dell'abilitazione del mouse
            bool isMouseTrackingEnabled = (chkMouse.Checked);

            // Variabile di controllo dell'abilitazione del debug
            bool isDebugEnabled = (chkDebug.Checked);

            // Stringa dedicata al log
            string sLog1 = "\r\n" + MaxIxCoords;
            string sLog2 = "\r\n" + MaxIxCoords;
            string sLog3 = "\r\n" + MaxIxCoords;
            string sLog4 = "\r\n" + MaxIxCoords;

            // Stats, azzero le statistiche correnti.
            // Le statistiche Overall non vengono toccate.
            statsMaxDistance = double.MinValue;
            statsMinDistance = double.MaxValue;
            statsCountDistances = 0;
            statsSumDistances = 0;

            // Disegno dei punti, solamente se la distanza è superiore allo 0
            for (int i = 0; i < MaxIxCoords; i++)
            {
                if (Coords[i] != null && Coords[i].ls.Distance > 0)
                {

                    double Distance = Coords[i].ls.Distance;
                    double Angle = Coords[i].ls.Angle;

                    if (isDebugEnabled)
                    {
                        if (Angle < 90)
                        {
                            sLog1 += "\r\n" + Angle.ToString() + ";" + Distance.ToString() + ";";
                        }
                        else if (Angle < 180)
                        {
                            sLog2 += "\r\n" + Angle.ToString() + ";" + Distance.ToString() + ";";
                        }
                        else if (Angle < 270)
                        {
                            sLog3 += "\r\n" + Angle.ToString() + ";" + Distance.ToString() + ";";
                        }
                        else
                        {
                            sLog4 += "\r\n" + Angle.ToString() + ";" + Distance.ToString() + ";";
                        }
                    }


                    // Disegno la coordinata se è all'interno del raggio del radar
                    if (Distance <= (int)(MaxDistance / 2 * ZoomFactor))
                    {
                        Coords[i].Draw(
                            memDC,
                            Center,
                            ZoomFactor,
                            radShowDots.Checked,
                            Coords[(i == 0 ? MaxIxCoords - 1 : i - 1)].ls);
                    }

                    // Stats correnti
                    if (Distance < statsMinDistance) { statsMinDistance = Distance; }
                    if (Distance > statsMaxDistance) { statsMaxDistance = Distance; }
                    statsCountDistances += 1;
                    statsSumDistances += Distance;

                    // Stats overall
                    if (Distance < statsMinDistanceO) { statsMinDistanceO = Distance; }
                    if (Distance > statsMaxDistanceO) { statsMaxDistanceO = Distance; }
                    statsCountDistancesO += 1;
                    statsSumDistancesO += Distance;

                    // Verifica dell'alert
                    if (!(alertFromAngle == -1))
                    {
                        if (((alertCrossZero) && ((Angle >= alertFromAngle) || (Angle <= alertToAngle)) && (Distance <= alertMinDistance)) ||
                        ((!alertCrossZero) && (Angle >= alertFromAngle) && (Angle <= alertToAngle) && (Distance <= alertMinDistance)))
                        {
                            // Alert!
                            if (isDebugEnabled)
                            {
                                if (Angle < 90)
                                {
                                    sLog1 += "A";
                                }
                                else if (Angle < 180)
                                {
                                    sLog2 += "A";
                                }
                                else if (Angle < 270)
                                {
                                    sLog3 += "A";
                                }
                                else
                                {
                                    sLog4 += "A";
                                }
                            }

                            memDC.DrawEllipse(new Pen(Color.Red, 3), (Center.X) - (MaxDistance / 2), (Center.Y) - (MaxDistance / 2), MaxDistance, MaxDistance);
                        }

                    }

                    // Verifico se è a portata di mouse
                    if ((isMouseTrackingEnabled) && (Angle <= MouseAngle))
                    {
                        if (isDebugEnabled)
                        {
                            if (Angle < 90)
                            {
                                sLog1 += "M" + MaxDistance.ToString();
                            }
                            else if (Angle < 180)
                            {
                                sLog2 += "M" + MaxDistance.ToString();
                            }
                            else if (Angle < 270)
                            {
                                sLog3 += "M" + MaxDistance.ToString();
                            }
                            else
                            {
                                sLog4 += "M" + MaxDistance.ToString();
                            }
                        }

                        // La coordinata Coords[400] contiene le informazioni del punto selezionato con il mouse
                        Coords[400] = Coords[i];

                    }

                }

            }


            // Linea del mouse
            if (isMouseTrackingEnabled && (Coords[400] != null))
            {
                double Distance = Coords[400].ls.Distance;
                double Angle = Coords[400].ls.Angle;

                //double distPixel = (Distance <= (int)(MaxDistance / 2 * ZoomFactor) ? Distance : (int)(MaxDistance / 2 * ZoomFactor)) * Convert.ToDouble(ZoomFactor);
                double distPixel = Distance / Convert.ToDouble(ZoomFactor);
                double rad = Angle * Math.PI / 180;
                int X = (int)(Math.Sin(rad) * (distPixel) + Center.X);
                int Y = (int)(Center.Y - Math.Cos(rad) * (distPixel));

                memDC.DrawLine(pMouse, Center.X, Center.Y, X, Y);

            }
            else if (isMouseTrackingEnabled)
            {
                double Distance = MaxDistance / 2;
                double Angle = MouseAngle;

                double distPixel = Distance;
                double rad = Angle * Math.PI / 180;
                int X = (int)(Math.Sin(rad) * (distPixel) + Center.X);
                int Y = (int)(Center.Y - Math.Cos(rad) * (distPixel));

                memDC.DrawLine(pMouse, Center.X, Center.Y, X, Y);

            }

            // Aggiornamento statistiche
            this.Invoke((MethodInvoker)delegate
            {
                // Mouse
                if (isMouseTrackingEnabled && (Coords[400] != null))
                {
                    lblDistance.Text = Math.Round(Coords[400].ls.Distance, 2).ToString("n2") + "cm";
                    lblDegrees.Text = Math.Round(Coords[400].ls.Angle, 2).ToString("n2") + "°";
                    lblMouse.Text = Math.Round(MouseAngle, 2).ToString("n2") + "°";
                }

                // Debug
                if (isDebugEnabled)
                {
                    txtLog1.Text = sLog1;
                    txtLog2.Text = sLog2;
                    txtLog3.Text = sLog3;
                    txtLog4.Text = sLog4;

                }

                // Stats
                lblMinDistance.Text = (statsMaxDistance < 0 ? "/" : statsMinDistance.ToString("n2") + "cm");
                lblMaxDistance.Text = (statsMaxDistance < 0 ? "/" : statsMaxDistance.ToString("n2") + "cm");
                lblAvgDistance.Text = (statsMaxDistance < 0 ? "/" : (statsSumDistances / statsCountDistances).ToString("n2") + "cm");

                lblMinDistanceO.Text = (statsMaxDistanceO < 0 ? "/" : statsMinDistanceO.ToString("n2") + "cm");
                lblMaxDistanceO.Text = (statsMaxDistanceO < 0 ? "/" : statsMaxDistanceO.ToString("n2") + "cm");
                lblAvgDistanceO.Text = (statsMaxDistanceO < 0 ? "/" : (statsSumDistancesO / statsCountDistancesO).ToString("n2") + "cm");

                // Alert
                lblAlertFrom.Text = (alertFromAngle == -1 ? "/" : alertFromAngle.ToString("n2") + "°");
                lblAlertTo.Text = (alertFromAngle == -1 ? "/" : alertToAngle.ToString("n2") + "°");
                lblAlertMinDistance.Text = (alertFromAngle == -1 ? "/" : alertMinDistance.ToString("n2") + "cm");

                // Zoom
                lblZoomFactor.Text = ZoomFactor.ToString() + "x";

            });

            // Rilascio immagine
            Graphics clientDC = this.CreateGraphics();

            IntPtr hdc = clientDC.GetHdc();
            IntPtr hMemdc = memDC.GetHdc();
            Win32Support.BitBlt(hdc, RadarLocation.X, RadarLocation.Y, RadarSize.Width, RadarSize.Height,
                hMemdc, 0, 0, Win32Support.TernaryRasterOperations.SRCCOPY);

            clientDC.ReleaseHdc(hdc);
            memDC.ReleaseHdc(hMemdc);

            isDrawing = false;

        }

        public void AddPoint(LidarSurvey survey)
        {

            // Evito il disegno dei punti se non ho un flag di nuova rilevazione
            if (survey.S == 1)
            {
                // Se non sto già disegnando allora chiamo il disegno
                if (!isDrawing)
                {
                    MaxIxCoords = ixCoords;
                    ixCoords = 0;
                    Draw();

                }

            }
            else if (ixCoords == -1)
            {
                // La procedura non è ancora inizializzata, non faccio nulla
                // e resto in attesa del flag di nuova rilevazione
                return;
            }

            // Aggiorno la collezione degli oggetti con le informazioni sulle misure
            Coords[ixCoords] = new Coordinates(survey);

            // Gestisco i contatori
            ixCoords++;
            if (ixCoords == 361) ixCoords = 0;

        }

        private void pictureBox1_MouseMove(object sender, MouseEventArgs e)
        {
            // Calcolo l'arcotangente del mouse
            if (e.X - Center.X - RadarLocation.X >= 0)
            {
                MouseAngle = Math.Atan2(e.X - Center.X - RadarLocation.X, -(e.Y - Center.Y - RadarLocation.Y)) * (180 / Math.PI);
            }
            else
            {
                MouseAngle = Math.Atan2(e.X - Center.X - RadarLocation.X, -(e.Y - Center.Y - RadarLocation.Y)) * (180 / Math.PI) + 360;
            }

            // Forzo il ridisegno
            if (!isDrawing) Draw();

        }

        //#endregion

        private class Coordinates
        {
            public LidarSurvey ls { get; private set; }

            public Coordinates(LidarSurvey ls)
            {
                this.ls = ls;
            }

            public void Draw(Graphics g, Point Center, decimal Zoom, bool ShowAsDots, LidarSurvey sPrec)
            {
                // Imposto il colore in base alla distanza dato per
                // scontato che il massimo raggiungibile è 400
                // 0 è completamente rosso
                // 200 è completamente giallo
                // 400 è cmpletamente verde
                int R = 0;
                int G = 0;
                int B = 0;
                if (ls.Distance < 200)
                {
                    R = 255;
                    G = Convert.ToInt32(ls.Distance * 200 / 255);
                    B = 0;

                }
                else if (ls.Distance > 400)
                {
                    R = 0;
                    G = Convert.ToInt32(255 - R);
                    B = 0;
                }
                else
                {
                    R = Convert.ToInt32(255 - ((ls.Distance - 200) * 200 / 255));
                    G = 255;
                    B = 0;
                }

                Color c = Color.FromArgb(255, R, G, B);

                // Calcolo le coordinate del punto corrente
                double distPixel = ls.Distance * Convert.ToDouble(Zoom);
                double rad = ls.Angle * Math.PI / 180;
                double X = Math.Sin(rad) * (distPixel) + Center.X;
                double Y = Center.Y - Math.Cos(rad) * (distPixel);

                if (ShowAsDots)
                {
                    g.FillEllipse(new SolidBrush(c), (int)X, (int)Y, 3, 3);

                }
                else
                {
                    // Calcolo le coordinate del punto precedente, se devo disegnare le linee
                    double distPixelPrec = sPrec.Distance * Convert.ToDouble(Zoom);
                    double radPrec = sPrec.Angle * Math.PI / 180;
                    double XPrec = Math.Sin(radPrec) * (distPixelPrec) + Center.X;
                    double YPrec = Center.Y - Math.Cos(radPrec) * (distPixelPrec);

                    if (ls.Distance == 0 || sPrec.Distance == 0)
                    {
                        g.FillEllipse(new SolidBrush(c), (int)X, (int)Y, 3, 3);
                    }
                    else
                    {
                        g.DrawLine(new Pen(c), (int)X, (int)Y, (int)XPrec, (int)YPrec);
                    }

                }

            }

        }

        private void btnZoomPlus_Click(object sender, EventArgs e)
        {
            ZoomFactor += (ZoomFactor == Convert.ToDecimal(3) ? 0 : Convert.ToDecimal(0.1));
            CreateBase();
            if (!isDrawing) Draw();

        }

        private void btnZoomMinus_Click(object sender, EventArgs e)
        {
            ZoomFactor -= (ZoomFactor == Convert.ToDecimal(0.3) ? 0 : Convert.ToDecimal(0.1));
            CreateBase();
            if (!isDrawing) Draw();

        }

        private void btnSetAlerts_Click(object sender, EventArgs e)
        {
            // Alert
            frmAlerts frm = new frmAlerts();
            frm.ShowDialog();

            SetAlert(frm.MinAngle, frm.MaxAngle, frm.MinDistance);
            CreateBase();
            if (!isDrawing) Draw();

        }

        private void chkDebug_CheckedChanged(object sender, EventArgs e)
        {
            txtLog1.Visible = (chkDebug.Checked);
            txtLog2.Visible = (chkDebug.Checked);
            txtLog3.Visible = (chkDebug.Checked);
            txtLog4.Visible = (chkDebug.Checked);
        }

        private void chkShowDrone_CheckedChanged(object sender, EventArgs e)
        {
            picDrone.Size = new Size(
                    Convert.ToInt32(txtDroneSize.Text.Split(new Char[] { ';' })[0]),
                    Convert.ToInt32(txtDroneSize.Text.Split(new Char[] { ';' })[1])
                    );
            CreateBase();
            if (!isDrawing) Draw();

        }

    }

}